Approximate Inverse Kinematics using Jacobian
Jacky Baltes
National Taiwan Normal University
Taipei, Taiwan
jacky.baltes@ntnu.edu.tw
09 May 2022
# These libraries provide us with the most often used features
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
import numpy as np
import math
import random
import jb_geometry as jbg
Robotis Thormang Arms Kinematics
import jb_geometry as jbg
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)
def thormang_forward_kinematics( thetas, ax = None, frame_in = None ):
frame = []
# Draw the world coordinate system
frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) )
# Draw the left shoulder Joint 1 (Lateral)
leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
j1A = leftShoulderTranslation.dot( leftShoulderRotation )
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A )
j2_d = 0.1 # m
j2_a = 0.05 # m
j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )
j2A = j1A.dot(j2T)
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )
#jbg.drawLink(ax, j1A, j2A, width=25 )
j3_d = 0.2
j3_a = 0.05
# Converted into one rotation around Z and one translation along X
j3_dp = 0.0
j3_ap = math.hypot( j3_d, j3_a)
j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )
j3A = j2A.dot(j3T)
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )
# Joint 4
j4_d = 0.22
j4_a = -0.05
# Convert into one translation along the X axis
j4_dp = 0.0
j4_ap = math.hypot( j4_d, j4_a)
j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )
#Breaking Denavit Hartenberg Convention
j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )
j4A = j3A.dot(j4T)
j4Ad = j3A.dot( j4Td )
frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
#jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
if ( frame_in is not None ):
frame_in.extend( frame )
return j4A
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi] )
fig5 = addJBFigure("fig5", 0, 0, fig )
from matplotlib.animation import ArtistAnimation
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi] )
frames = []
for i in range(45):
frame = []
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0+i*2/180.0*math.pi, 45.0/180.0*math.pi], frame )
#ax.view_init(elev=10, azim = i * 4 )
frames.append( frame )
ani = ArtistAnimation( fig, frames, interval=100)
a1 = addJBAnimation("a1", 0, 0, ani )
plt.close()
import jb_geometry as jbg
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)
def thormang_forward_kinematics( thetas, ax = None, frame_in = None ):
frame = []
# Draw the world coordinate system
frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) )
# Draw the left shoulder Joint 1 (Lateral)
leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
j1A = leftShoulderTranslation.dot( leftShoulderRotation )
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A )
j2_d = 0.1 # m
j2_a = 0.05 # m
j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )
j2A = j1A.dot(j2T)
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )
#jbg.drawLink(ax, j1A, j2A, width=25 )
j3_d = 0.2
j3_a = 0.05
# Converted into one rotation around Z and one translation along X
j3_dp = 0.0
j3_ap = math.hypot( j3_d, j3_a)
j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )
j3A = j2A.dot(j3T)
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )
# Joint 4
j4_d = 0.22
j4_a = -0.05
# Convert into one translation along the X axis
j4_dp = 0.0
j4_ap = math.hypot( j4_d, j4_a)
j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )
#Breaking Denavit Hartenberg Convention
j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )
j4A = j3A.dot(j4T)
j4Ad = j3A.dot( j4Td )
frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
#jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
if ( frame_in is not None ):
frame_in.extend( frame )
return j4A
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi] )
fig5 = addJBFigure("fig5", 0, 0, fig )
import jb_geometry as jbg
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)
def thormang_forward_kinematics( thetas, ax = None, frame_in = None ):
frame = []
# Draw the world coordinate system
frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) )
# Draw the left shoulder Joint 1 (Lateral)
leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
j1A = leftShoulderTranslation.dot( leftShoulderRotation )
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )
j2_d = 0.1 # m
j2_a = 0.05 # m
j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )
j2A = j1A.dot(j2T)
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )
#jbg.drawLink(ax, j1A, j2A, width=25 )
j3_d = 0.2
j3_a = 0.05
# Converted into one rotation around Z and one translation along X
j3_dp = 0.0
j3_ap = math.hypot( j3_d, j3_a)
j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )
j3A = j2A.dot(j3T)
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )
# Joint 4
j4_d = 0.22
j4_a = -0.05
# Convert into one translation along the X axis
j4_dp = 0.0
j4_ap = math.hypot( j4_d, j4_a)
j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )
#Breaking Denavit Hartenberg Convention
j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )
j4A = j3A.dot(j4T)
j4Ad = j3A.dot( j4Td )
frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
#jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
if ( frame_in is not None ):
frame_in.extend( frame )
return j4A
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi] )
fig5 = addJBFigure("fig5", 0, 0, fig )
import jb_geometry as jbg
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)
def thormang_forward_kinematics( thetas, ax = None, frame_in = None ):
frame = []
# Draw the world coordinate system
frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) )
# Draw the left shoulder Joint 1 (Lateral)
leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
j1A = leftShoulderTranslation.dot( leftShoulderRotation )
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )
j2_d = 0.1 # m
j2_a = 0.05 # m
j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )
j2A = j1A.dot(j2T)
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )
#jbg.drawLink(ax, j1A, j2A, width=25 )
j3_d = 0.2
j3_a = 0.05
# Converted into one rotation around Z and one translation along X
j3_dp = 0.0
j3_ap = math.hypot( j3_d, j3_a)
j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )
j3A = j2A.dot(j3T)
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )
# Joint 4
j4_d = 0.22
j4_a = -0.05
# Convert into one translation along the X axis
j4_dp = 0.0
j4_ap = math.hypot( j4_d, j4_a)
j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )
#Breaking Denavit Hartenberg Convention
j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )
j4A = j3A.dot(j4T)
j4Ad = j3A.dot( j4Td )
frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
#jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
if ( frame_in is not None ):
frame_in.extend( frame )
return j4A
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax )
fig5 = addJBFigure("fig5", 0, 0, fig )
import jb_geometry as jbg
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)
def thormang_forward_kinematics( thetas, ax = None, frame_in = None ):
frame = []
# Draw the world coordinate system
frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) )
# Draw the left shoulder Joint 1 (Lateral)
leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
j1A = leftShoulderTranslation.dot( leftShoulderRotation )
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )
j2_d = 0.1 # m
j2_a = 0.05 # m
j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )
j2A = j1A.dot(j2T)
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )
#jbg.drawLink(ax, j1A, j2A, width=25 )
j3_d = 0.2
j3_a = 0.05
# Converted into one rotation around Z and one translation along X
j3_dp = 0.0
j3_ap = math.hypot( j3_d, j3_a)
j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )
j3A = j2A.dot(j3T)
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )
# Joint 4
j4_d = 0.22
j4_a = -0.05
# Convert into one translation along the X axis
j4_dp = 0.0
j4_ap = math.hypot( j4_d, j4_a)
j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )
#Breaking Denavit Hartenberg Convention
j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )
j4A = j3A.dot(j4T)
j4Ad = j3A.dot( j4Td )
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
#jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
if ( frame_in is not None ):
frame_in.extend( frame )
return j4A
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax )
fig5 = addJBFigure("fig5", 0, 0, fig )
from matplotlib.animation import ArtistAnimation
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi] )
frames = []
for i in range(45):
frame = []
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0+i*2/180.0*math.pi, 45.0/180.0*math.pi], frame )
#ax.view_init(elev=10, azim = i * 4 )
frames.append( frame )
ani = ArtistAnimation( fig, frames, interval=100)
a1 = addJBAnimation("a1", 0, 0, ani )
plt.close()
import jb_geometry as jbg
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)
def thormang_forward_kinematics( thetas, ax = None, frame_in = None ):
frame = []
# Draw the world coordinate system
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) )
# Draw the left shoulder Joint 1 (Lateral)
leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
j1A = leftShoulderTranslation.dot( leftShoulderRotation )
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )
j2_d = 0.1 # m
j2_a = 0.05 # m
j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )
j2A = j1A.dot(j2T)
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )
#jbg.drawLink(ax, j1A, j2A, width=25 )
j3_d = 0.2
j3_a = 0.05
# Converted into one rotation around Z and one translation along X
j3_dp = 0.0
j3_ap = math.hypot( j3_d, j3_a)
j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )
j3A = j2A.dot(j3T)
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )
# Joint 4
j4_d = 0.22
j4_a = -0.05
# Convert into one translation along the X axis
j4_dp = 0.0
j4_ap = math.hypot( j4_d, j4_a)
j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )
#Breaking Denavit Hartenberg Convention
j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )
j4A = j3A.dot(j4T)
j4Ad = j3A.dot( j4Td )
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
#jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
if ( frame_in is not None ):
frame_in.extend( frame )
return j4A
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax )
fig5 = addJBFigure("fig5", 0, 0, fig )
from matplotlib.animation import ArtistAnimation
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi] )
frames = []
for i in range(45):
frame = []
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0+i*2/180.0*math.pi, 45.0/180.0*math.pi], frame )
#ax.view_init(elev=10, azim = i * 4 )
frames.append( frame )
ani = ArtistAnimation( fig, frames, interval=100)
a1 = addJBAnimation("a1", 0, 0, ani )
plt.close()
import jb_geometry as jbg
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)
def thormang_forward_kinematics( thetas, **, ax = None, frame_in = None ):
frame = []
# Draw the world coordinate system
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) )
# Draw the left shoulder Joint 1 (Lateral)
leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
j1A = leftShoulderTranslation.dot( leftShoulderRotation )
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )
j2_d = 0.1 # m
j2_a = 0.05 # m
j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )
j2A = j1A.dot(j2T)
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )
#jbg.drawLink(ax, j1A, j2A, width=25 )
j3_d = 0.2
j3_a = 0.05
# Converted into one rotation around Z and one translation along X
j3_dp = 0.0
j3_ap = math.hypot( j3_d, j3_a)
j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )
j3A = j2A.dot(j3T)
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )
# Joint 4
j4_d = 0.22
j4_a = -0.05
# Convert into one translation along the X axis
j4_dp = 0.0
j4_ap = math.hypot( j4_d, j4_a)
j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )
#Breaking Denavit Hartenberg Convention
j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )
j4A = j3A.dot(j4T)
j4Ad = j3A.dot( j4Td )
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
#jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
if ( frame_in is not None ):
frame_in.extend( frame )
return j4A
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax )
fig5 = addJBFigure("fig5", 0, 0, fig )
import jb_geometry as jbg
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)
def thormang_forward_kinematics( thetas, *, ax = None, frame_in = None ):
frame = []
# Draw the world coordinate system
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) )
# Draw the left shoulder Joint 1 (Lateral)
leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
j1A = leftShoulderTranslation.dot( leftShoulderRotation )
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )
j2_d = 0.1 # m
j2_a = 0.05 # m
j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )
j2A = j1A.dot(j2T)
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )
#jbg.drawLink(ax, j1A, j2A, width=25 )
j3_d = 0.2
j3_a = 0.05
# Converted into one rotation around Z and one translation along X
j3_dp = 0.0
j3_ap = math.hypot( j3_d, j3_a)
j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )
j3A = j2A.dot(j3T)
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )
# Joint 4
j4_d = 0.22
j4_a = -0.05
# Convert into one translation along the X axis
j4_dp = 0.0
j4_ap = math.hypot( j4_d, j4_a)
j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )
#Breaking Denavit Hartenberg Convention
j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )
j4A = j3A.dot(j4T)
j4Ad = j3A.dot( j4Td )
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
#jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
if ( frame_in is not None ):
frame_in.extend( frame )
return j4A
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax )
fig5 = addJBFigure("fig5", 0, 0, fig )
import jb_geometry as jbg
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)
def thormang_forward_kinematics( thetas, *, ax = None, frame_in = None ):
frame = []
# Draw the world coordinate system
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) )
# Draw the left shoulder Joint 1 (Lateral)
leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
j1A = leftShoulderTranslation.dot( leftShoulderRotation )
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )
j2_d = 0.1 # m
j2_a = 0.05 # m
j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )
j2A = j1A.dot(j2T)
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )
#jbg.drawLink(ax, j1A, j2A, width=25 )
j3_d = 0.2
j3_a = 0.05
# Converted into one rotation around Z and one translation along X
j3_dp = 0.0
j3_ap = math.hypot( j3_d, j3_a)
j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )
j3A = j2A.dot(j3T)
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )
# Joint 4
j4_d = 0.22
j4_a = -0.05
# Convert into one translation along the X axis
j4_dp = 0.0
j4_ap = math.hypot( j4_d, j4_a)
j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )
#Breaking Denavit Hartenberg Convention
j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )
j4A = j3A.dot(j4T)
j4Ad = j3A.dot( j4Td )
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
#jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
if ( frame_in is not None ):
frame_in.extend( frame )
return j4A
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax=ax )
fig5 = addJBFigure("fig5", 0, 0, fig )
Thormang Forward Kinematics
Thormang Forward Kinematics
Thormang Forward Kinematics
Thormang Forward Kinematics
Thormang Forward Kinematics
Thormang Forward Kinematics
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])
print(n1)
print(pos_end_effector1[0:3])
[[-8.66025404e-01 4.53153894e-01 2.11309131e-01 6.37498350e-02] [-4.34682641e-17 -4.22618262e-01 9.06307787e-01 7.05649271e-01] [ 5.00000000e-01 7.84885567e-01 3.65998151e-01 5.10417953e-01] [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]] [0.08488075 0.79628005 0.54701777]
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.extend( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
Thormang Inverse Kinematics (Iterative)
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
print(len(frames))
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
45
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
17
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
#frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
#frames.append( frame )
print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
#frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
#frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
15
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
17
import jb_geometry as jbg
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)
def thormang_forward_kinematics( thetas, *, A = None, ax = None, frame_in = None ):
frame = []
# Draw the world coordinate system
if A is None:
A = np.eye(4)
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0, A ) )
# Draw the left shoulder Joint 1 (Lateral)
leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
j1A = A.dot( leftShoulderTranslation.dot( leftShoulderRotation ) )
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, A.dot( j1A ) ) )
j2_d = 0.1 # m
j2_a = 0.05 # m
j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )
j2A = j1A.dot(j2T)
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )
#jbg.drawLink(ax, j1A, j2A, width=25 )
j3_d = 0.2
j3_a = 0.05
# Converted into one rotation around Z and one translation along X
j3_dp = 0.0
j3_ap = math.hypot( j3_d, j3_a)
j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )
j3A = j2A.dot(j3T)
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )
# Joint 4
j4_d = 0.22
j4_a = -0.05
# Convert into one translation along the X axis
j4_dp = 0.0
j4_ap = math.hypot( j4_d, j4_a)
j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )
#Breaking Denavit Hartenberg Convention
j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )
j4A = j3A.dot(j4T)
j4Ad = j3A.dot( j4Td )
if ax is not None:
frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
#jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
if ( frame_in is not None ):
frame_in.extend( frame )
return j4A
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax=ax )
fig5 = addJBFigure("fig5", 0, 0, fig )
Thormang Forward Kinematics
from matplotlib.animation import ArtistAnimation
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0+i*2/180.0*math.pi, 45.0/180.0*math.pi], ax=ax, frame_in=frame )
#ax.view_init(elev=10, azim = i * 4 )
#print('frame', frame )
frames.append( frame.copy() )
#print('frames', frames )
ani = ArtistAnimation( fig, frames, interval=100)
a1 = addJBAnimation("a1", 0, 0, ani )
plt.close()
Thormang Forward Kinematics
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])
print(n1)
print(pos_end_effector1[0:3])
[[-8.66025404e-01 4.53153894e-01 2.11309131e-01 6.37498350e-02] [-4.34682641e-17 -4.22618262e-01 9.06307787e-01 7.05649271e-01] [ 5.00000000e-01 7.84885567e-01 3.65998151e-01 5.10417953e-01] [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]] [0.08488075 0.79628005 0.54701777]
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
A = rotate_z( i * 4/180.0 * math.pi )
a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
A = rotateZ( i * 4/180.0 * math.pi )
a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
A = jbg.rotate_z( i * 4/180.0 * math.pi )
a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
17
Thormang Inverse Kinematics (Iterative)
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
A = jbg.rotate_z( i * 4/180.0 * math.pi )
a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
pos_end_effector = a_03.dot( [ 0, 0, 0.1, 1 ] )
a_target = A.dot( target )
frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
17
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
A = jbg.rotate_z( i * 4/180.0 * math.pi )
a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
pos_end_effector = a_03.dot( [ 0, 0, 0.1, 1 ] )
a_target = A.dot( target )
frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
Thormang Inverse Kinematics (Iterative)
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
A = jbg.rotate_z( -i * 2/180.0 * math.pi )
a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
pos_end_effector = a_03.dot( [ 0, 0, 0.1, 1 ] )
a_target = A.dot( target )
frame.extend( ax.plot([ pos_end_effector1[0], a_target[0]], [ pos_end_effector1[1], a_target[1]], [ pos_end_effector1[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
Thormang Inverse Kinematics (Iterative)
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0,1])
solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range(45):
frame = []
A = jbg.rotate_z( -i * 2/180.0 * math.pi )
a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
pos_end_effector = a_03.dot( [ 0, 0, 0.1, 1 ] )
a_target = A.dot( target )
frame.extend( ax.plot([ pos_end_effector[0], a_target[0]], [ pos_end_effector[1], a_target[1]], [ pos_end_effector[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
ani = ArtistAnimation( fig, frames, interval=100)
a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
Thormang Inverse Kinematics (Iterative)
Iterative Solution to Inverse Kinematics
Assume: complex robot system (e.g., humanoid from foot to arm, snake like robot) that we cannot break down into subchains that we can reduce to a two-link problem
Then: We can establish an error criteria (Euclidean distance of end effector to target location)
Iteratively approach the target location by moving closer toward the target in small steps
If current position of end effector is already close to the target location
Direction of Movement in each Iteration
Current configuration: θ_1 .. θ_n
Current end effector position: c_x, c_y, c_z. Solve forward kinematics problem
Rigid robots can easily solve the forward kinematics problem using DHH convention. Much harder for soft robots
How do we change the current configuration (joint angles) to move closer to the target?
Iterative Inverse Kinematics
Approch is to reduce the error. Multivariate optimization problem - Define error function (e.g., Euclidan distance to goal) and then minimize the error function
Minimize the error function -> calculate the derivative of the error function
Some applications (e.g., linear regression), we can calculate the derivative of the error function analytically. In more complex cases, we can calculate the derivative of the error function numerically. In this case, we find an approximate solution
Calculating the analytical solution may be too time consuming (inverting a large matix). Sometimes, we use an approximation instead of an analytical solution. This is especially true for problems with many dimensions (hundreds or thousands). Matrix inversion is O(n**3)
Approximation Algorithm
Move each joint individually and then check how the error changes
Numerically approximate the first (partial) derivative of the multivariate error function with respect to the current configuration θ_1 .. θ_n
Gradient of the error function
\[ \nabla e(\theta_1, \theta_2, \cdots, \theta_n ) = [ \frac{\delta e}{\delta \theta_1}, \frac{\delta e}{\delta \theta_2}, \cdots, \frac{\delta e}{\delta \theta_n} ] \]Gradient descent algorithm - take a small step in the direction opposite of the gradient (minimize error)
\[ \theta_1 = \theta_1 - \alpha \frac{\delta e}{\delta \theta_1}, \theta_2 = \theta_2 - \alpha \frac{\delta e}{\delta \theta_2}, \cdots, \theta_n = \theta_n - \alpha \frac{\delta e}{\delta \theta_n} \] , where \( \alpha \) is a small constant \( ( 0 < \alpha \leq 1 ) \)
Gradient Descent Algorithm
Gradient descent algorithm can be written in matrix form
\[ \theta^{t+1} = \theta^{t} - \alpha \nabla e(\theta) \]
Jacobian
In inverse kinematics, the error consists of 3 components \( ( \Delta x, \Delta y, \Delta z ) \)
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
def fwd_kinematics( thetas ):
print("fwd_kinematics")
e = thormang_forward_kinematics(thetas).dot([0.0, 0.0, 0.1, 1] )
return e
def Jacobian( thetas, fkin ):
J = np.zeros( (3, len(thetas) ) )
current_pos = fkin( thetas )
dt = 1.0/180.0*math.pi
for i,t in enumerate(thetas):
thetas_d = thetas.copy()
thetas_d[i] = thetas_d[i] + dt
p = fkin( thetas_d )
# print('t_d', thetas_d )
# print('p', p)
# print('J', J)
J[0:3,i] = ( p[0:3] - current_pos[0:3] ) / dt
return J
jac = Jacobian( current_angles, fwd_kinematics )
print("Current Angles", current_angles)
print("Current Position", current_position)
print('Jacobian\n', jac)
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics Current Angles [0.5235987755982988, 0.4363323129985824, 1.3962634015954636] Current Position [0.14268179 0.39627408 0.64713211 1. ] Jacobian [[ 0.24587446 0.03417197 -0.06684805] [ 0. -0.33596852 -0.29499608] [-0.14483112 0.05918759 -0.11578422]]
The Jaobian
The Jaobian provides us with the following information
Jacobian at position: ['30.00 deg.', '25.00 deg.', '80.00 deg.']
| θ_1 | θ_2 | θ_3 | |
|---|---|---|---|
| X | 0.25 | 0.03 | -0.07 |
| Y | 0.00 | -0.34 | -0.29 |
| Z | -0.14 | 0.06 | -0.12 |
Jacobian Solution
The following matrix equation relates \( \delta err \) with \( \delta \theta \)
\[ \delta err = J \delta \theta \]Solve the equation by calculating the inverse of the Jacobian
\[ \delta \theta = J^{-1} \delta err \]The Jacobian is usually not square (3 rows, # joints columns) and even if square, it may be singular
Apply the Moore Penrose Pseudo Inverse method (pre-multiply by the transpose of the Jacobian) \( J^{T} \)
\[ J^T \delta err = J^T J \delta \theta \\ (J^T J)^{-1} J^T \delta err = (J^T J)^{-1} (J^T J) \delta \theta \\ (J^T J)^{-1} J^T \delta err = \delta \theta \]
# Pseudo Inverse method
current_angles = np.array( solution1 )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
d_err = (target - current_position)[0:3]
alpha = 0.3
count = 0
errs = []
while( np.linalg.norm(d_err) > 0.1 ):
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
d_err = (target - current_position)[0:3]
errs.append( np.linalg.norm(d_err) )
print("d_err", np.linalg.norm(d_err), "target", target, "current_position", current_position )
jac = Jacobian(current_angles, fwd_kinematics)
#print("Jacobian", jac )
d_theta = np.linalg.inv( jac.T.dot(jac) ).dot( jac.T).dot(d_err)
#print("d_theta", d_theta )
#print("current_angles.shape", current_angles.shape)
current_angles = current_angles + alpha * d_theta
#print("current_angles.shape", current_angles.shape)
#print("current_angles", current_angles )
count = count + 1
if (count > 40 ):
break
fig = plt.figure()
ax = fig.add_subplot(1,1,1)
ax.plot(errs, "b-", linewidth=2)
e1 = addJBFigure("e1", 0, 0, fig )
plt.close()
d_err 0.6803841190440875 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.14268179 0.39627408 0.64713211 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.539377462685629 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.06912383 0.42159308 0.50169066 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.42302129012094863 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.00150497 0.47190383 0.4014226 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.40001725789713116 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.04978688 0.55773932 0.4599157 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.33144358985326183 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.02397445 0.61317654 0.39929704 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.30046636828614787 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.00445318 0.66487258 0.39922975 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.28110233673490215 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.0035492 0.7025137 0.39601305 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.20232447004242446 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.01655725 0.72182565 0.3198601 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.14136374442265123 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.03648416 0.73723855 0.26347302 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.10038365163024875 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.04667577 0.75028648 0.22560372 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.07246167693708219 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.0534846 0.7604632 0.20026643 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, target, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
frames.append( frame )
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
def fwd_kinematics( thetas, *, A = None, ax = None, frame_in = None ):
print("fwd_kinematics")
e = thormang_forward_kinematics(thetas, A=A, ,ax=ax, frame_in=frame_in).dot([0.0, 0.0, 0.1, 1] )
return e
def Jacobian( thetas, fkin ):
J = np.zeros( (3, len(thetas) ) )
current_pos = fkin( thetas )
dt = 1.0/180.0*math.pi
for i,t in enumerate(thetas):
thetas_d = thetas.copy()
thetas_d[i] = thetas_d[i] + dt
p = fkin( thetas_d )
# print('t_d', thetas_d )
# print('p', p)
# print('J', J)
J[0:3,i] = ( p[0:3] - current_pos[0:3] ) / dt
return J
jac = Jacobian( current_angles, fwd_kinematics )
print("Current Angles", current_angles)
print("Current Position", current_position)
print('Jacobian\n', jac)
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
def fwd_kinematics( thetas, *, A = None, ax = None, frame_in = None ):
print("fwd_kinematics")
e = thormang_forward_kinematics(thetas, A=A,ax=ax, frame_in=frame_in).dot([0.0, 0.0, 0.1, 1] )
return e
def Jacobian( thetas, fkin ):
J = np.zeros( (3, len(thetas) ) )
current_pos = fkin( thetas )
dt = 1.0/180.0*math.pi
for i,t in enumerate(thetas):
thetas_d = thetas.copy()
thetas_d[i] = thetas_d[i] + dt
p = fkin( thetas_d )
# print('t_d', thetas_d )
# print('p', p)
# print('J', J)
J[0:3,i] = ( p[0:3] - current_pos[0:3] ) / dt
return J
jac = Jacobian( current_angles, fwd_kinematics )
print("Current Angles", current_angles)
print("Current Position", current_position)
print('Jacobian\n', jac)
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics Current Angles [0.5235987755982988, 0.4363323129985824, 1.3962634015954636] Current Position [0.14268179 0.39627408 0.64713211 1. ] Jacobian [[ 0.24587446 0.03417197 -0.06684805] [ 0. -0.33596852 -0.29499608] [-0.14483112 0.05918759 -0.11578422]]
The Jaobian
The Jaobian provides us with the following information
Jacobian at position: ['30.00 deg.', '25.00 deg.', '80.00 deg.']
| θ_1 | θ_2 | θ_3 | |
|---|---|---|---|
| X | 0.25 | 0.03 | -0.07 |
| Y | 0.00 | -0.34 | -0.29 |
| Z | -0.14 | 0.06 | -0.12 |
Jacobian Solution
The following matrix equation relates \( \delta err \) with \( \delta \theta \)
\[ \delta err = J \delta \theta \]Solve the equation by calculating the inverse of the Jacobian
\[ \delta \theta = J^{-1} \delta err \]The Jacobian is usually not square (3 rows, # joints columns) and even if square, it may be singular
Apply the Moore Penrose Pseudo Inverse method (pre-multiply by the transpose of the Jacobian) \( J^{T} \)
\[ J^T \delta err = J^T J \delta \theta \\ (J^T J)^{-1} J^T \delta err = (J^T J)^{-1} (J^T J) \delta \theta \\ (J^T J)^{-1} J^T \delta err = \delta \theta \]
# Pseudo Inverse method
current_angles = np.array( solution1 )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
d_err = (target - current_position)[0:3]
alpha = 0.3
count = 0
errs = []
while( np.linalg.norm(d_err) > 0.1 ):
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
d_err = (target - current_position)[0:3]
errs.append( np.linalg.norm(d_err) )
print("d_err", np.linalg.norm(d_err), "target", target, "current_position", current_position )
jac = Jacobian(current_angles, fwd_kinematics)
#print("Jacobian", jac )
d_theta = np.linalg.inv( jac.T.dot(jac) ).dot( jac.T).dot(d_err)
#print("d_theta", d_theta )
#print("current_angles.shape", current_angles.shape)
current_angles = current_angles + alpha * d_theta
#print("current_angles.shape", current_angles.shape)
#print("current_angles", current_angles )
count = count + 1
if (count > 40 ):
break
fig = plt.figure()
ax = fig.add_subplot(1,1,1)
ax.plot(errs, "b-", linewidth=2)
e1 = addJBFigure("e1", 0, 0, fig )
plt.close()
d_err 0.6803841190440875 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.14268179 0.39627408 0.64713211 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.539377462685629 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.06912383 0.42159308 0.50169066 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.42302129012094863 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.00150497 0.47190383 0.4014226 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.40001725789713116 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.04978688 0.55773932 0.4599157 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.33144358985326183 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.02397445 0.61317654 0.39929704 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.30046636828614787 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.00445318 0.66487258 0.39922975 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.28110233673490215 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.0035492 0.7025137 0.39601305 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.20232447004242446 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.01655725 0.72182565 0.3198601 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.14136374442265123 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.03648416 0.73723855 0.26347302 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.10038365163024875 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.04667577 0.75028648 0.22560372 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.07246167693708219 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.0534846 0.7604632 0.20026643 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, target, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
frames.append( frame )
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.1
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
frames.append( frame )
d_err = (target - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6171148590704281 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5602591787176366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5079162799036441 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.45915126894154806 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.41433425819284286 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.37500733211206727 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34669374072592596 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.33413184288406866 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3228893421701253 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.32024359057275675 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3080615201700689 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2871093154347471 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.25888571704116103 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23286732447681371 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20955015449909548 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18861526520041552 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16980974306606894 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15290904460171195 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13771347248722277 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12404552162852442 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11174731299726161 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10067824175841517 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09071289949610226 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08173926785436081 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07365715566055361 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06637684571359614 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05981791933024497 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.053908231084458365 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.048583010763466626 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.043784073636250975 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03945912350771325 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03556113575491366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03204780971521377 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02888108153014193 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.026026689938875004 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02345378863816573 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0211345997409344 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.019044103618979052 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.017159761040851578 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.015461264040069292 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.013930312391442566 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.012550412949812002 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.01130669942781996 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.010185770466930479 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.1
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye()
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target )
frame.extend( ax.plot([ pos_end_effector[0], a_target[0]], [ pos_end_effector[1], a_target[1]], [ pos_end_effector[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
d_err = (target - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.1
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target )
frame.extend( ax.plot([ pos_end_effector[0], a_target[0]], [ pos_end_effector[1], a_target[1]], [ pos_end_effector[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frames.append( frame )
d_err = (target - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6171148590704281 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5602591787176366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5079162799036441 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.45915126894154806 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.41433425819284286 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.37500733211206727 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34669374072592596 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.33413184288406866 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3228893421701253 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.32024359057275675 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3080615201700689 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2871093154347471 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.25888571704116103 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23286732447681371 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20955015449909548 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18861526520041552 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16980974306606894 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15290904460171195 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13771347248722277 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12404552162852442 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11174731299726161 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10067824175841517 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09071289949610226 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08173926785436081 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07365715566055361 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06637684571359614 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05981791933024497 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.053908231084458365 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.048583010763466626 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.043784073636250975 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03945912350771325 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03556113575491366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03204780971521377 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02888108153014193 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.026026689938875004 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02345378863816573 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0211345997409344 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.019044103618979052 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.017159761040851578 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.015461264040069292 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.013930312391442566 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.012550412949812002 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.01130669942781996 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.010185770466930479 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.1
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target )
frame.extend( ax.plot([ pos_end_effector[0], a_target[0]], [ pos_end_effector[1], a_target[1]], [ pos_end_effector[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6171148590704281 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5602591787176366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5079162799036441 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.45915126894154806 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.41433425819284286 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.37500733211206727 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34669374072592596 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.33413184288406866 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3228893421701253 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.32024359057275675 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3080615201700689 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2871093154347471 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.25888571704116103 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23286732447681371 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20955015449909548 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18861526520041552 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16980974306606894 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15290904460171195 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13771347248722277 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12404552162852442 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11174731299726161 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10067824175841517 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09071289949610226 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08173926785436081 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07365715566055361 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06637684571359614 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05981791933024497 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.053908231084458365 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.048583010763466626 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.043784073636250975 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03945912350771325 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03556113575491366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03204780971521377 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02888108153014193 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.026026689938875004 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02345378863816573 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0211345997409344 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.019044103618979052 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.017159761040851578 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.015461264040069292 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.013930312391442566 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.012550412949812002 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.01130669942781996 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.010185770466930479 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.1
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6171148590704281 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5602591787176366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5079162799036441 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.45915126894154806 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.41433425819284286 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.37500733211206727 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34669374072592596 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.33413184288406866 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3228893421701253 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.32024359057275675 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3080615201700689 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2871093154347471 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.25888571704116103 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23286732447681371 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20955015449909548 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18861526520041552 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16980974306606894 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15290904460171195 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13771347248722277 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12404552162852442 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11174731299726161 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10067824175841517 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09071289949610226 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08173926785436081 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07365715566055361 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06637684571359614 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05981791933024497 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.053908231084458365 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.048583010763466626 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.043784073636250975 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03945912350771325 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03556113575491366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03204780971521377 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02888108153014193 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.026026689938875004 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02345378863816573 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0211345997409344 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.019044103618979052 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.017159761040851578 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.015461264040069292 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.013930312391442566 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.012550412949812002 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.01130669942781996 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.010185770466930479 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.2, 0.6, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.1
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6171148590704281 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5602591787176366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5079162799036441 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.45915126894154806 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.41433425819284286 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.37500733211206727 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34669374072592596 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.33413184288406866 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3228893421701253 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.32024359057275675 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3080615201700689 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2871093154347471 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.25888571704116103 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23286732447681371 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20955015449909548 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18861526520041552 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16980974306606894 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15290904460171195 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13771347248722277 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12404552162852442 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11174731299726161 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10067824175841517 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09071289949610226 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08173926785436081 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07365715566055361 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06637684571359614 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05981791933024497 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.053908231084458365 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.048583010763466626 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.043784073636250975 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03945912350771325 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03556113575491366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03204780971521377 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02888108153014193 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.026026689938875004 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02345378863816573 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0211345997409344 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.019044103618979052 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.017159761040851578 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.015461264040069292 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.013930312391442566 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.012550412949812002 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.01130669942781996 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.010185770466930479 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.2, 0.6, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.1
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6171148590704281 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5602591787176366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5079162799036441 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.45915126894154806 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.41433425819284286 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.37500733211206727 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34669374072592596 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.33413184288406866 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3228893421701253 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.32024359057275675 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3080615201700689 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2871093154347471 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.25888571704116103 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23286732447681371 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20955015449909548 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18861526520041552 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16980974306606894 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15290904460171195 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13771347248722277 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12404552162852442 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11174731299726161 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10067824175841517 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09071289949610226 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08173926785436081 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07365715566055361 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06637684571359614 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05981791933024497 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.053908231084458365 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.048583010763466626 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.043784073636250975 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03945912350771325 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03556113575491366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03204780971521377 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02888108153014193 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.026026689938875004 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02345378863816573 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0211345997409344 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.019044103618979052 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.017159761040851578 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.015461264040069292 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.013930312391442566 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.012550412949812002 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.01130669942781996 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.010185770466930479 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.2, 0.6, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.1
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6171148590704281 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5602591787176366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5079162799036441 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.45915126894154806 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.41433425819284286 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.37500733211206727 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34669374072592596 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.33413184288406866 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3228893421701253 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.32024359057275675 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3080615201700689 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2871093154347471 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.25888571704116103 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23286732447681371 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20955015449909548 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18861526520041552 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16980974306606894 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15290904460171195 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13771347248722277 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12404552162852442 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11174731299726161 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10067824175841517 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09071289949610226 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08173926785436081 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07365715566055361 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06637684571359614 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05981791933024497 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.053908231084458365 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.048583010763466626 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.043784073636250975 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03945912350771325 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03556113575491366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03204780971521377 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02888108153014193 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.026026689938875004 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02345378863816573 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0211345997409344 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.019044103618979052 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.017159761040851578 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.015461264040069292 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.013930312391442566 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.012550412949812002 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.01130669942781996 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.010185770466930479 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.2, 0.6, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.1
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.29380026601450127 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2651871485914366 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23924422542888069 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21573752277975952 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1944609816267056 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17522452717642426 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15784944827785663 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14216774335742954 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12802276222389447 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1152697469940783 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10377586903387712 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09341983174914892 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08409122145398475 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07568975198217288 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06812448701855986 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.061313080035714146 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05518104843775663 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.04966108838932524 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0446924329144104 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.04022025427966999 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03619511077062393 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.032572437214964056 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.029312077933743894 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02637786024678644 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02373720625493644 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.021360780371437505 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.019222169958699596 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.017297596416621007 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.015565654135138045 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.014007074841196917 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.012604515018572486 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.011342364242818748 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.010206572442557989 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.009184494265207057
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.5, 0.7, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.1
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.47759521031616575 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4297922860394924 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.38661204494647383 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34782131180937714 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3130786871311263 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.28200570860377633 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2542357020161144 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2294402051745769 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20734512016444775 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18775400813263857 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17062154773372695 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1564142584972816 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15042105988219276 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15940560262623146 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1468416332953813 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14425917913453273 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14573413327683793 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13934308311030083 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2897045377393598 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2618440410687554 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23682876110989673 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21438951325885722 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1943046548089601 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17641001321114638 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16063780860704832 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14717048060437218 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13739314193112528 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16563228391594728 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1512842145566422 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1397963283070927 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1380179146518387 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14520090393795487 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13620627709919664 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21087328495353225 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19116211287184248 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1736177148068067 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15819129736570625 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14512674028250427 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13623018839863663 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21982574756636436 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19915567643339913 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18071205121759965 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16438992677671183 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15026401890040897 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13909937368961442
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.5, 0.7, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.25
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4045671431083639 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3044800338620111 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23199465654408294 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1805779541070138 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14688217196056932 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16621926269202328 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13893305529660363 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23980013468427452 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1878105266587548 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15122064884507389 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1368537500252748 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3887675630186442 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2987770337895822 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23160915379081484 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1819885130207531 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14767814141232105 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14200038582642377 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16199139513131555 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1371430650350476 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.32417123824186317 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.25050691774508743 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19586404865348173 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1567320730362408 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1365439124879922 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3973536165494696 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3014451484414789 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23226617755186307 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18203652355112312 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.148352595528219 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15996330923600863 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13710577655159734 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 1.0965669190792435 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5295248642785918 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4184428152189937 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3183211437780796 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.24542643707741113 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19182731590375865 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15419584951401089 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14063014975787155 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.24950825072234772 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19513921420949518 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1562538621177821 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1366317908331659 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.8188631991270038 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6717984777136295
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.5, 0.7, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.15
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.45214968620079027 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3842613471515385 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3265872201058371 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2781762996938153 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23769453064475077 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20395603409518553 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.176178744953095 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1545634334643768 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14767999380189542 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15578592195129026 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14040328340960423 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.22664498901025656 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1954826622084345 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16955540555622398 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14882410114610292 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13625819893388044 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.625523468787739 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.8075179888332358 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.706442004259637 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6148707931635683 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5249174525168404 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.44656223279285495 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3812769027974004 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3262820665889081 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2797556797428292 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2403727810314921 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.207121399065079 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17928180013299236 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15660001141692387 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14057598090916168 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17535428251470733 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.153187156513759 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1378085246843571 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17968241892255693 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15676903169390238 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13983258074594057 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14679834706269285 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13572489734760987 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.9314789526738959 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.7811956234532861 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6646960953288696 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5768406512407355 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.22230268373351464 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19111798899847962 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17261286678220175
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.5, 0.7, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.05
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5036770607261558 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4784987547429801 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.45452636379039946 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.43172380964568985 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4100512631712042 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3894656636781167 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3699217602660103 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3513732333095507 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.33377368473874014 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.31707742533964123 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3012400591208827 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2862188967457927 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.271973241473044 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.25846459371905206 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2456568217296666 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23351635210382293 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.22201245325659655 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.211117733926127 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20080909935068775 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19106972097208388 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18189348615537768 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17329652708465293 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16535427670130884 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15837528343887283 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15490661789744425 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1589255053583814 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1522894661113319 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1486619790130483 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17394814704284114 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1657355634896333 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.158059876160979 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15100219793901531 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14486366424913438 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14169901136281962 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18997582658182416 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18085405903592916 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17223431333049435 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1641115749114096 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15649732837144031 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14944142160273635 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14311154657675368 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.138272436018892 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.152134173588734 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14543329086090556 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13966599871035118
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.5, 0.7, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.01
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5247972771326571 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5195520978331117 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5143583445697565 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5092155597236728 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5041232897285112 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4990810847742677 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.49408849854769343 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.489145088007263 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4842504131904142 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.47940403705062984 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4746055253218561 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.46985444640773477 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.46515037129315867 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4604928734757308 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4558815289148077 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.45131591599594056 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4467956155086629 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4423202106357309 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.43788928695207996 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4335024324319164 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.42915923746252466 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.42485929486350926 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4206021999103471 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4163875503612478 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4122149464864472 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4080839910991754 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.40399428958764044 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.39994544994746617 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3959370828141019 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.39196880149480506 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3880402219998563 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3841509630727313 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.38030064621900384 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.376488895733802 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.37271533872767587 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3689796051507762 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3652813278152647 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.36162014241591345 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.35799568754886457 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.35440760472854627 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.35085553840275235 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34733913596591626 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.343858047770609 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34041192713731333 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3370004303625262
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.5, 0.7, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.02
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.519504920615106 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5091206439832692 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4989382319361884 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.48895444228275897 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4791660592848198 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4695698838935475 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4601627275863288 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4509414091413999 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.44190275371293225 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4330435936297849 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4243607704205687 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.41585113765326953 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.40751156426063134 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.39933893809747156 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3913301695405428 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3834821949948331 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3757919802128858 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3682565233670602 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3608728578400887 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.35363805471830206 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34654922498578605 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3396035214276905 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.33279814025782495 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3261303224903369 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.31959735507827186 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.31319657184364497 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3069253542246662 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3007811318663015 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2947613830806446 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.28886363520385056 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2830854648768519 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2774244982779464 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.27187841133683255 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.26644492996206426 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.261121830317514 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2559069391887739 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2507981344880814 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2457933459572723 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.24089055614372873 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2360878017462591 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23138317545924994 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2267748284887365 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.22226097398029704 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2178398916970414 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21350993443474944
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.5, 0.7, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.02
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 70 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.519504920615106 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5091206439832692 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4989382319361884 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.48895444228275897 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4791660592848198 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4695698838935475 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4601627275863288 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4509414091413999 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.44190275371293225 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4330435936297849 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4243607704205687 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.41585113765326953 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.40751156426063134 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.39933893809747156 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3913301695405428 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3834821949948331 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3757919802128858 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3682565233670602 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3608728578400887 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.35363805471830206 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34654922498578605 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3396035214276905 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.33279814025782495 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3261303224903369 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.31959735507827186 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.31319657184364497 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3069253542246662 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3007811318663015 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2947613830806446 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.28886363520385056 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2830854648768519 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2774244982779464 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.27187841133683255 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.26644492996206426 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.261121830317514 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2559069391887739 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2507981344880814 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2457933459572723 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.24089055614372873 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2360878017462591 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23138317545924994 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2267748284887365 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.22226097398029704 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2178398916970414 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21350993443474944 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20926953689101702 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20511722806879995 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20105164888569899 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19707157765117858 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19317596779513108 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1893640053459877 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18563519956827093 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18198953203948717 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17842771494676354 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17495166889902175 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17156548471410576 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16827759109196244 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16510648539706704 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16210009142144216 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15943719652525404 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15919144511017574 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15692352108230762 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20998251904396073 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2058220077132482 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20174686039138093 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1977555563542425 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19384664580394073 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19001875843605295 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18627061533331385 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18260104566915852
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.5, 0.7, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.035
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 70 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5115797567573875 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4936826656827211 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.47638870064014643 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4596833451190141 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.44355172320194075 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4279785414768303 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4129481314485645 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3984445474239693 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3844516876436552 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3709534180524315 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3579336869677975 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3453766249705965 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3332666281686309 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.32158842523525705 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3103271298500571 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.29946828076440934 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.288997871951686 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.27890237536357365 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2691687588321664 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.25978450173988377 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2507376113367347 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.24201664316450733 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.233610730188172 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.22550962736756405 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21770378236717813 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21018445063196414 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20294388794991705 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19597568478983124 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18927537685531545 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18284163949884896 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17667885550035048 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17080341883273875 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16526258740161812 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16021266069481593 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15656458617694272 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.8933729501771535 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.861621500391537 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.8311528964286548 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.8019194419280213 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.7739602977589151 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.7477676317685317 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.7298368824741681 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.7156529391693222 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.7037798954810436 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6990920415574864 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.7117514305252326 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6982809011838838 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6762690296810232 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6537218107662768 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6310009496814344 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.6118536605428496 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5903858432049177 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5697099029927063 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5497949162033311 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5306026640393319 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.5120998645668792 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4942575021284819 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4770493686334467 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4604511823494841 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.44444010901855174 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.42899449245969323 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.41409369132533846 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.39971797153760397 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.38584842957200016 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3724669339231653 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.35955607799723827 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3470991406583499 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.33508005222962056 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3234833646182771 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.31229422474001656
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.5, 0.3, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.1
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 70 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.400914290636055 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.36067559945073174 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3241355641870142 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2911484438727125 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2614993604404171 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23491635534686311 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21110579893057746 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18978029298768714 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17067333442429072 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15354455042623325 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13817994048795657 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12438996760320369 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11200696301728197 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1008825018607927 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09088500374112778 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08189762991503721 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07381647398796652 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06654901766978996 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.060012819307999926 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05413440884026634 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.04884837485354633 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0440966499347214 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.039828040544340915 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03599814150199943 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.032570037108809444 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02951707438208652 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.026832823396395294 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.024578084768723824 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02337164083274722 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03033932633163899 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.027530829586948597 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.025056158156155587 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.022951598232604453 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.021504800555920592 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.043779282974366056 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.039597670144300226 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03583549280049822 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03245537920004466 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.029426068241693748 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02672436266839663 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.024341375839484987 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.022306606089976092 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.020838835714644746 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0259752275868011 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.023686172437226448 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.021767002433611463 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.020599479722649996 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2218631396787895 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20015160018286163 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18058825724385077 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16295603349281165 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.147061302060217 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1327307922832322 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11980914624518536 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10815688971030091 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09764869962660315 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08817190105340482 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07962515086731403 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07191727893712889 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06496626568555124 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05869834084761614 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.053047193373354756 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.047953288167947146 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.043363293672519974 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03922963912574416 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03551025131738925 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.032168594147856486 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.029174332928789203 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02650557296127787 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.024156075080956154
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.5, 0.3, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.05
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 45 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4228306773278025 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4016975622537616 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3815563486605925 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.36237305449101315 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34411518590633167 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3267501789166017 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3102446403100308 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.29456419930768035 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2796737408647584 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.26553782012588817 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.25212111617682537 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23938884243918865 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.22730707661107863 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21584300218927668 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20496506920628105 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19464308826533264 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18484827310205434 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1755532454390088 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1667320134645302 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15835993273001614 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.150413656014022 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1428710768751508 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13571127020608792 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1289144320510193 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12246182018655694 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11633569642452475 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11051927121672071 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10499665088226866 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09975278760353573 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09477343222181248 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09004508979102574 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08555497780384821 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08129098698072033 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0772416445023826 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07339607956643347 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0697439911556715 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06627561791925649 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06298171008684846 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05985350336177115 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.056882694774280866 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05406142052454169 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05138223591457777 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.04883809757245078 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.046422348333322705 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.044128705402599666
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.5, 0.3, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.05
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 70 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4228306773278025 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4016975622537616 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3815563486605925 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.36237305449101315 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34411518590633167 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3267501789166017 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3102446403100308 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.29456419930768035 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2796737408647584 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.26553782012588817 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.25212111617682537 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23938884243918865 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.22730707661107863 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21584300218927668 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20496506920628105 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19464308826533264 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18484827310205434 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1755532454390088 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1667320134645302 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15835993273001614 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.150413656014022 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1428710768751508 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13571127020608792 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1289144320510193 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12246182018655694 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11633569642452475 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11051927121672071 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10499665088226866 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09975278760353573 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09477343222181248 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09004508979102574 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08555497780384821 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08129098698072033 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0772416445023826 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07339607956643347 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0697439911556715 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06627561791925649 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06298171008684846 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05985350336177115 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.056882694774280866 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05406142052454169 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05138223591457777 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.04883809757245078 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.046422348333322705 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.044128705402599666 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.04195125286385002 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.039884440360004846 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03792309117923309 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03606242568250453 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.034298111550260184 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.032626364514104444 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.031044152520109714 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0295496349036038 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.028143212706960512 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0268304994417576 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.025633387203730103 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.024660285732514314 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.027363978067538004 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.026079202575769355 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02488657033726163 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.023816114547724834 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.023030696301796 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.04772047336037443 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.045379396804967304 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0431553547731283 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.04104266534045308 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03903596077951482 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03713018111985442 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03532057208529972 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.033602689413755334
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.5, 0.3, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.02
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 70 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4361087195011584 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.42739520092682864 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.41884994519552954 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.41046998414833125 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4022524411746021 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3941945179288535 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3862934823730568 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.37854665826195305 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3709514161269173 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3635051657557301 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3562053501149881 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3490494406211251 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34203493363611576 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.33515934804472747 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3284202237607471 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3218151210084693 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3153416202312063 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.30899732248889483 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3027798502204286 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.29668684826169106 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.29071598502623974 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.28486495377131915 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2791314738866807 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.27351329215717907 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.26800818396202947 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.26261395438389135 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.25732843920957305 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2521495058112607 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.24707505390286155 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.24210301617050736 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23723135877964652 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23245808176362678 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.22778121930040487 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.22319883988514164 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21870904640708583 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21430997613942465 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20999980065076584 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20577672564669733 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20163899074951125 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19758486922371044 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19361266765439591 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18972072558508088 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18590741512091302 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1821711405027336 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17851033765686908 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17492347372503697 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17140904657827835 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16796558431838432 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16459164476987787 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16128581496524363 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15804671062576167 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1548729756400006 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15176328154174992 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14871632698893483 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1457308372448345 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14280556366273803 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1399392831750003 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1371307977873127 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1343789340788678 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1316825427089883 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12904049793068428 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1264516971115168 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12391506026207043 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1214295295722653 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11899406895569166 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11660766360208893 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11426931953805365 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11197806319602722 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10973294099157374 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10753301890894114
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position = [ 0.5, 0.3, 0.4, 1 ]
# while( math.hypot( current_position - target ) > eps ):
# # move current_angles
# current_angles = inverse_kinematics_step(current_angles)
# current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)
def inverse_kinematics_step( thetas, alpha, target, fkin ):
current_position = fkin( thetas )
d_err = (target - current_position)[0:3]
J = Jacobian( thetas, fkin )
d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err)
thetas = thetas + alpha * d_theta
return thetas
eps = 0.01
alpha = 0.04
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))
frames = []
for i in range( 70 ):
frame = []
A = np.eye(4)
a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
a_target = A.dot( target_position )
frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )
frames.append( frame )
d_err = (target_position - current_position)[0:3]
print('err', np.linalg.norm( d_err ) )
if np.linalg.norm( d_err ) < eps:
break
ani = ArtistAnimation( fig, frames, interval=100)
a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.427248545333861 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.4101714270327574 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3937409639386623 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3779376256918557 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.36274289451176367 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.34813872614862 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3341071917948226 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3206302826605762 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.3076898436917607 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2952675979268856 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.283345225697623 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2719044698194792 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.260927246136014 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2503957463555324 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.2402925261448335 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.23060057572643106 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.22130337296413982 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.21238492046991433 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.20382976898324276 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.19562302946505558 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18775037623520272 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.18019804321845673 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.17295281504459958 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.16600201442878232 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.15933348696755512 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1529355842356561 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14679714586108872 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.14090748108845594 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.13525635020795138 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1298339461241817 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.12463087625965699 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11963814492752411 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11484713626282267 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.11024959776786698 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.1058376245026108 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.10160364393291016 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09754040143682305 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.09364094646019322 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08989861930679952 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08630703854457221 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.08286008900726023 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07955191037004106 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07637688627766671 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07332963400459311 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.07040499462808845 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06759802369747066 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06490398238547578 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.06231832911138062 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.059836711630158315 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.057454959587968146 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.055169077552280454 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.05297523853576866 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.050869778048177716 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.04884918873185757 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.046910115668050915 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.04504935248806578 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.043263838495802794 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.041550657122410364 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.039907036219881134 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03833035101318468 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03681813107603778 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03536807368735076 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.03397806781863398 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.032646236828468005 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0313710162194182 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.0301513022994921 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.028986758575354973 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.027878519877879235 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.026831090769848288 fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics err 0.02585892050707842
Cheating with Jacobians
Since the Jacobian already estimates how much the target position will change, we can cheat a little bit with respect to calculating the pseudo inverse of the Jacobian
We can use the transpose of the Jacobian directly.
Much simpler and faster to compute that calculating the pseudo inverse. No need to invert the matrix
current_angles = np.array( solution1 )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
target_angles = np.array( solution2 )
target_position = thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
d_err = (target - current_position)[0:3]
alpha = 0.5
count = 0
errs = []
while( np.linalg.norm(d_err) > 0.1 ):
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
d_err = (target - current_position)[0:3]
errs.append( np.linalg.norm(d_err) )
print("d_err", np.linalg.norm(d_err), "target", target, "current_position", current_position )
jac = Jacobian(current_angles, fwd_kinematics)
d_theta = jac.T.dot(d_err)
current_angles = current_angles + alpha * d_theta
count = count + 1
if (count > 40 ):
break
fig = plt.figure()
ax = fig.add_subplot(1,1,1)
ax.plot(errs, "b-", linewidth=2)
e2 = addJBFigure("e2", 0, 0, fig )
plt.close()
d_err 0.6803841190440875 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.14268179 0.39627408 0.64713211 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.6556877831256608 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.14272912 0.43168698 0.64124858 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.6275884590936451 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.14076922 0.46957246 0.63215939 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.5958663769474063 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.13620501 0.50921155 0.61922448 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.5605446657146683 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.12851698 0.54951159 0.60200832 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.5219699943239683 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.11738686 0.58907181 0.58041933 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.48085065571913777 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.1028155 0.62634976 0.55480763 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.43822474048676946 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.08518878 0.6599036 0.52597257 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.39535071615695483 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.06525349 0.68864129 0.49505858 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.3535427021310278 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.04399917 0.71199147 0.46336436 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.3139963456625801 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.02248373 0.72994042 0.43212972 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.2776531669797927 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [0.00166362 0.74293878 0.40236797 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.24513109620720508 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.01772182 0.75173196 0.3747824 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.21672111533963306 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.03519396 0.75718159 0.3497645 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.19243055493165862 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.05051863 0.7601259 0.32744603 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.1720488056850544 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.06365588 0.76129577 0.30777203 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.15521683074486808 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.07470011 0.76128051 0.29057099 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.14149033740115458 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.08382601 0.76052745 0.27561074 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.13039263807323392 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.09124682 0.75935963 0.2626375 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.12145596831712868 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.09718562 0.75799998 0.2514004 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.11425081147626118 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.10185759 0.75659545 0.24166512 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.10840340461071328 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.10546023 0.7552378 0.2332204 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.10360271280710157 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.10816907 0.75398016 0.22588007 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics d_err 0.09959929732136613 target [-0.06981941 0.79628005 0.1394304 1. ] current_position [-0.11013661 0.75284956 0.21948249 1. ] fwd_kinematics fwd_kinematics fwd_kinematics fwd_kinematics
